import time, os, sys
import math
import cv_lite  # 导入cv_lite扩展模块
import ulab.numpy as np  # 导入numpy库
from media.sensor import *
from media.display import *
from media.media import *
from machine import UART, FPIOA
from machine import Pin

# --------------------------- 硬件初始化 ---------------------------
# 串口初始化
# 配置引脚
fpioa = FPIOA()
fpioa.set_function(11, FPIOA.UART2_TXD)
fpioa.set_function(12, FPIOA.UART2_RXD)
# 初始化UART2，波特率115200，8位数据位，无校验，1位停止位
uart_x = UART(UART.UART2, baudrate=38400, bits=UART.EIGHTBITS, parity=UART.PARITY_NONE, stop=UART.STOPBITS_ONE)


fpioa.set_function(50, FPIOA.UART3_TXD)
fpioa.set_function(51, FPIOA.UART3_RXD)
# 初始化UART2，波特率115200，8位数据位，无校验，1位停止位
uart_y = UART(UART.UART3, baudrate=38400, bits=UART.EIGHTBITS, parity=UART.PARITY_NONE, stop=UART.STOPBITS_ONE)

# 激光控制引脚
fpioa.set_function(42,FPIOA.GPIO42)
LED = Pin(42, Pin.OUT, pull=Pin.PULL_DOWN, drive=7)  # 蓝灯
LED.low()  #关闭激光


# 屏幕分辨率设置
lcd_width = 800
lcd_height = 480

# 摄像头初始化（注意：保留RGB模式用于色块检测，后续转为灰度图用于矩形检测）
sensor = Sensor(id = 2)
sensor.reset()
sensor.set_framesize(width=320, height=240)  # 降低分辨率提高帧率
sensor.set_pixformat(Sensor.RGB565)  # 保留彩色用于紫色色块检测

# 显示初始化
Display.init(Display.ST7701, width=lcd_width, height=lcd_height, to_ide=True)
MediaManager.init()
sensor.run()

# --------------------------- 配置参数 ---------------------------
# 矩形检测核心参数（基于cv_lite）
canny_thresh1      = 50        # Canny边缘检测低阈值
canny_thresh2      = 150       # Canny边缘检测高阈值
approx_epsilon     = 0.04      # 多边形拟合精度（越小越精确）
area_min_ratio     = 0.005     # 最小面积比例（相对于图像总面积）
max_angle_cos      = 0.3       # 角度余弦阈值（越小越接近矩形）
gaussian_blur_size = 3         # 高斯模糊核尺寸（奇数）

# 原有筛选参数
MIN_AREA = 100               # 最小面积阈值
MAX_AREA = 100000             # 最大面积阈值
MIN_ASPECT_RATIO = 0.3        # 最小宽高比
MAX_ASPECT_RATIO = 3.0        # 最大宽高比

# 虚拟坐标与圆形参数
BASE_RADIUS = 45              # 基础半径（虚拟坐标单位）
POINTS_PER_CIRCLE = 24        # 圆形采样点数量
PURPLE_THRESHOLD = (20, 60, 15, 70, -70, -20)  # 紫色色块阈值

# 基础矩形参数（固定方向，不再自动切换）
RECT_WIDTH = 210    # 固定矩形宽度
RECT_HEIGHT = 95    # 固定矩形高度
# 移除自动切换方向的逻辑，始终使用固定宽高的虚拟矩形

# --------------------------- 工具函数 ---------------------------
def calculate_distance(p1, p2):
    return math.sqrt((p2[0] - p1[0])**2 + (p2[1] - p1[1])** 2)

def calculate_center(points):
    if not points:
        return (0, 0)
    sum_x = sum(p[0] for p in points)
    sum_y = sum(p[1] for p in points)
    return (sum_x / len(points), sum_y / len(points))

def is_valid_rect(corners):
    edges = [calculate_distance(corners[i], corners[(i+1)%4]) for i in range(4)]

    # 对边比例校验
    ratio1 = edges[0] / max(edges[2], 0.1)
    ratio2 = edges[1] / max(edges[3], 0.1)
    valid_ratio = 0.5 < ratio1 < 1.5 and 0.5 < ratio2 < 1.5

    # 面积校验
    area = 0
    for i in range(4):
        x1, y1 = corners[i]
        x2, y2 = corners[(i+1) % 4]
        area += (x1 * y2 - x2 * y1)
    area = abs(area) / 2
    valid_area = MIN_AREA < area < MAX_AREA

    # 宽高比校验
    width = max(p[0] for p in corners) - min(p[0] for p in corners)
    height = max(p[1] for p in corners) - min(p[1] for p in corners)
    aspect_ratio = width / max(height, 0.1)
    valid_aspect = MIN_ASPECT_RATIO < aspect_ratio < MAX_ASPECT_RATIO

    return valid_ratio and valid_area and valid_aspect

def detect_purple_blobs(img):
    return img.find_blobs(
        [PURPLE_THRESHOLD],
        pixels_threshold=100,
        area_threshold=100,
        merge=True
    )

def send_circle_points(points):
    if not points:
        return
    count = len(points)
    msg = f"$$C,{count},"
    for x, y in points:
        msg += f"{x},{y},"
    msg = msg.rstrip(',') + "##"
    uart.write(msg)
    # print(f"发送圆形点: {msg}")

def get_perspective_matrix(src_pts, dst_pts):
    """计算透视变换矩阵"""
    A = []
    B = []
    for i in range(4):
        x, y = src_pts[i]
        u, v = dst_pts[i]
        A.append([x, y, 1, 0, 0, 0, -u*x, -u*y])
        A.append([0, 0, 0, x, y, 1, -v*x, -v*y])
        B.append(u)
        B.append(v)

    # 高斯消元求解矩阵
    n = 8
    for i in range(n):
        max_row = i
        for j in range(i, len(A)):
            if abs(A[j][i]) > abs(A[max_row][i]):
                max_row = j
        A[i], A[max_row] = A[max_row], A[i]
        B[i], B[max_row] = B[max_row], B[i]

        pivot = A[i][i]
        if abs(pivot) < 1e-8:
            return None
        for j in range(i, n):
            A[i][j] /= pivot
        B[i] /= pivot

        for j in range(len(A)):
            if j != i and A[j][i] != 0:
                factor = A[j][i]
                for k in range(i, n):
                    A[j][k] -= factor * A[i][k]
                B[j] -= factor * B[i]

    return [
        [B[0], B[1], B[2]],
        [B[3], B[4], B[5]],
        [B[6], B[7], 1.0]
    ]

def transform_points(points, matrix):
    """应用透视变换将虚拟坐标映射到原始图像坐标"""
    transformed = []
    for (x, y) in points:
        x_hom = x * matrix[0][0] + y * matrix[0][1] + matrix[0][2]
        y_hom = x * matrix[1][0] + y * matrix[1][1] + matrix[1][2]
        w_hom = x * matrix[2][0] + y * matrix[2][1] + matrix[2][2]
        if abs(w_hom) > 1e-8:
            transformed.append((x_hom / w_hom, y_hom / w_hom))
    return transformed

def sort_corners(corners):
    """将矩形角点按左上、右上、右下、左下顺序排序"""
    center = calculate_center(corners)
    sorted_corners = sorted(corners, key=lambda p: math.atan2(p[1]-center[1], p[0]-center[0]))

    # 调整顺序为左上、右上、右下、左下
    if len(sorted_corners) == 4:
        left_top = min(sorted_corners, key=lambda p: p[0]+p[1])
        index = sorted_corners.index(left_top)
        sorted_corners = sorted_corners[index:] + sorted_corners[:index]
    return sorted_corners

def get_rectangle_orientation(corners):
    """计算矩形的主方向角（水平边与x轴的夹角）"""
    if len(corners) != 4:
        return 0

    # 计算上边和右边的向量
    top_edge = (corners[1][0] - corners[0][0], corners[1][1] - corners[0][1])
    right_edge = (corners[2][0] - corners[1][0], corners[2][1] - corners[1][1])

    # 选择较长的边作为主方向
    if calculate_distance(corners[0], corners[1]) > calculate_distance(corners[1], corners[2]):
        main_edge = top_edge
    else:
        main_edge = right_edge

    # 计算主方向角（弧度）
    angle = math.atan2(main_edge[1], main_edge[0])
    return angle

def send_motor_speed(ser, direction, speed):
    """
    发送电机控制指令

    参数：
    ser: 已打开的串口对象
    direction: 方向 (0=正转, 1=反转)
    speed: 速度档位 (0-127)

    返回：
    发送的完整指令字节（用于调试）
    """
    # 参数校验
    if speed < 0 or speed > 127:
        raise ValueError("速度档位必须在0-127范围内")
    if direction not in (0, 1):
        raise ValueError("方向参数必须为0(正转)或1(反转)")

    # 构建数据字节（最高位=方向，低7位=速度）
    data_byte = (direction << 7) | (speed & 0x7F)

    # 构建完整指令帧（固定头 + 数据字节）
    command = bytes([0xE0, 0xF6, data_byte])

    # 通过串口发送
    ser.write(command)

def set_motor_step_division(ser, division):
    """
    设置步进电机的细分级别

    参数：
    ser: 已打开的串口对象
    division: 细分值 (1-256)

    返回：
    发送的完整指令字节（用于调试）
    """
    # 参数校验
    if division < 1 or division > 256:
        return

    # 构建细分值字节
    # 注意：00 表示 256 细分
    if division == 256:
        division_byte = 0x00
    else:
        division_byte = division

    # 构建完整指令帧（固定头 + 命令码 + 细分值）
    command = bytes([0xE0, 0x84, division_byte])

    # 通过串口发送
    ser.write(command)



def send_motor_position(ser, direction, speed, pulses = 128):
     """
     串口直接位置控制指令发送函数（严格匹配图片中的协议）
     :param ser: 已打开的Serial串口对象
     :param direction: 方向 0=正转 1=反转
     :param speed: 速度 0-127（7位）
     :param pulses: 脉冲数（16位无符号）
     """
     # 指令头（固定为E0 FD，如图片所示）
     header = bytes([0xE0, 0xFD])

     # 方向速度组合（最高位=方向，低7位=速度）
     dir_speed = ((direction & 0x01) << 7) | (speed & 0x7F)

     # 脉冲数拆分为高低字节（小端序，如图片中的0C 80示例）
     pulse_low = pulses & 0xFF
     pulse_high = (pulses >> 8) & 0xFF

     # 完整指令（6字节，格式完全匹配图片）
     command = header + bytes([
         dir_speed,    # 方向速度组合
         pulse_high,   # 脉冲高字节（如0C）
         pulse_low,    # 脉冲低字节（如80）
         0x00          # 保留位
     ])

     # 发送指令
     ser.write(command)
     time.sleep(0.01)


def send_motor_stop(ser):
    """
    发送电机停止指令(符合图片中的协议格式)

    参数：
    ser: 已初始化的串口对象

    返回：
    发送的完整指令字节（用于调试）
    """
    # 构建停止指令帧（固定指令头）
    stop_command = bytes([0xE0, 0xF7])

    # 通过串口发送
    ser.write(stop_command)

'''
def get_motor_commands(current_x, current_y, img_width=320, img_height=240):
    """
    激光云台控制函数（物理参数精确版）
    输入：
        current_x, current_y: 当前激光点坐标（像素）
        img_width, img_height: 图像分辨率（默认320x240）
    输出：
        (x方向, x速度, x脉冲), (y方向, y速度, y脉冲)
    """
    # 固定目标点（图像中心）
    TARGET_X, TARGET_Y = img_width//2, img_height//2

    # 硬件参数（根据实际测量调整）
    MAX_SPEED = 100               # 最大速度档位
    MIN_SPEED = 5                 # 最低速度档位
    PULSE_PER_DEGREE = 256/1.8    # 每度脉冲数 ≈142.22
    DEADZONE_PIXEL = 2            # 死区像素数
    WORKING_DISTANCE = 1.2        # 工作距离1.2米（取1-1.5米中间值）

    # 计算像素到角度的转换系数
    pixel_to_angle_x = 130 / img_width    # 每像素水平角度 ≈0.406°
    pixel_to_angle_y = 130 * (img_height/img_width) / img_height  # 垂直角度

    # 计算误差（像素）
    error_x = TARGET_X - current_x
    error_y = TARGET_Y - current_y

    # 初始化输出（停止状态）
    x_cmd = (0, 0, 0)
    y_cmd = (0, 0, 0)

    # X轴处理
    if abs(error_x) >= DEADZONE_PIXEL:
        # 计算需要转动的角度（度）
        angle_x = error_x * pixel_to_angle_x

        # 计算所需脉冲数
        pulses_x = int(abs(angle_x) * PULSE_PER_DEGREE)

        # 计算速度（非线性曲线）
        speed_x = min(MAX_SPEED,
                     MIN_SPEED + int(math.log(abs(error_x) + 1) * 15))

        x_cmd = (1 if error_x > 0 else 0, speed_x, pulses_x)

    # Y轴处理
    if abs(error_y) >= DEADZONE_PIXEL:
        angle_y = error_y * pixel_to_angle_y
        pulses_y = int(abs(angle_y) * PULSE_PER_DEGREE)
        speed_y = min(MAX_SPEED,
                     MIN_SPEED + int(math.log(abs(error_y) + 1) * 15))
        y_cmd = (1 if error_y > 0 else 0, speed_y, pulses_y)

    return x_cmd, y_cmd



def get_motor_commands(current_x, current_y, img_width=320, img_height=240):
    """
    激光云台精准控制函数（大偏差高速/小偏差微调）
    输入：
        current_x, current_y: 当前激光点坐标（像素）
        img_width, img_height: 图像分辨率
    输出：
        (x方向, x速度, x脉冲), (y方向, y速度, y脉冲)
        方向：1=正方向，0=反方向
    """
    # 固定目标点（图像中心）
    TARGET_X, TARGET_Y = img_width//2, img_height//2

    # 硬件参数配置
    MAX_SPEED = 100               # 最大速度档位
    MIN_SPEED = 5                 # 最低速度档位
    PULSE_PER_DEGREE = 256/1.8    # 142.22脉冲/度
    DEADZONE_PIXEL = 2            # 死区阈值（像素）

    # 视觉参数计算
    pixel_to_angle = 130 / img_width  # 每像素对应角度（水平）
    aspect_ratio = img_height / img_width
    pixel_to_angle_y = pixel_to_angle * aspect_ratio  # 垂直方向

    # 计算误差
    error_x = TARGET_X - current_x
    error_y = TARGET_Y - current_y

    # 初始化输出（停止状态）
    x_cmd = (0, 0, 0)
    y_cmd = (0, 0, 0)

    # X轴控制
    if abs(error_x) >= DEADZONE_PIXEL:
        # 大偏差高速模式（误差>20像素）
        if abs(error_x) > 20:
            speed_x = MAX_SPEED
            pulses_x = int(abs(error_x) * 5)  # 大脉冲快速接近
        # 小偏差精调模式
        else:
            speed_x = MIN_SPEED + int(abs(error_x) * 2)  # 低速
            pulses_x = max(1, int(abs(error_x) * 0.3))  # 小脉冲微调

        x_cmd = (1 if error_x > 0 else 0,
                min(speed_x, MAX_SPEED),
                min(pulses_x, 5000))  # 限制最大脉冲

    # Y轴控制（逻辑相同）
    if abs(error_y) >= DEADZONE_PIXEL:
        if abs(error_y) > 20:
            speed_y = MAX_SPEED
            pulses_y = int(abs(error_y) * 5)
        else:
            speed_y = MIN_SPEED + int(abs(error_y) * 2)
            pulses_y = max(1, int(abs(error_y) * 0.3))

        y_cmd = (1 if error_y > 0 else 0,
                min(speed_y, MAX_SPEED),
                min(pulses_y, 5000))

    return x_cmd, y_cmd
'''

# 全局状态变量用于存储PID控制器状态
class PIDState:
    def __init__(self):
        self.prev_error = 0.0
        self.integral = 0.0
        self.last_time = time.time()

# 为X轴和Y轴分别创建PID状态
x_pid = PIDState()
y_pid = PIDState()

def get_motor_commands(current_x, current_y, img_width=320, img_height=240):
    """
    激光云台双PID控制系统
    特点：
    - X轴和Y轴使用独立的PID控制器
    - 自适应误差大小调整
    - 防止积分饱和
    - 输出限制保护电机
    - 平滑的速度过渡

    输入参数：
    current_x, current_y - 当前激光点在图像中的坐标
    img_width, img_height - 图像尺寸（默认320x240）

    输出：
    (x_direction, x_speed, x_pulses), (y_direction, y_speed, y_pulses)
    """
    # 固定目标点（图像中心）
    y_offset = 12
    TARGET_X, TARGET_Y = img_width//2, img_height//2 - y_offset

    # PID参数配置（可针对每个轴独立调整）
    PID_PARAMS = {
        'x': {
            'kp': 1.0,    # 比例增益 - X轴
            'ki': 0.0,   # 积分增益 - X轴
            'kd': 0.0,   # 微分增益 - X轴
        },
        'y': {
            'kp': 0.00,    # 比例增益 - Y轴（通常比X轴稍小）
            'ki': 0.0,   # 积分增益 - Y轴
            'kd': 0.0,    # 微分增益 - Y轴（通常需要更高抑制振荡）
        },
        'common': {
            'max_integral': 120,   # 积分限幅防止饱和
            'min_speed': 5,       # 最小速度档位
            'max_speed': 120,      # 最大速度档位
            'deadzone': 1.2,       # 死区阈值(像素)
            'max_pulses': 3000,    # 最大输出脉冲数
            'min_pulses': 1,       # 最小输出脉冲数
            'output_scale': 4.0    # 输出缩放因子
        }
    }

    # 计算误差
    error_x = -TARGET_X + current_x
    error_y = TARGET_Y - current_y

    def pid_controller(error, axis_state, axis_params):
        """独立PID控制器实现"""
        # 获取当前时间并计算时间差
        current_time = time.time()
        dt = current_time - axis_state.last_time
        axis_state.last_time = current_time

        # 处理第一次调用或dt过小的情况
        if dt <= 0.001:
            dt = 0.01  # 避免除零错误

        # 死区处理 - 小误差不响应
        if abs(error) < PID_PARAMS['common']['deadzone']:
            # 重置积分项避免累积
            axis_state.integral *= 0.5  # 缓慢衰减积分项
            return (0, 0, 0)

        # P项: 比例控制
        p_term = axis_params['kp'] * error

        # I项: 积分控制 (带限幅)
        axis_state.integral += error * dt
        # 抗积分饱和处理
        max_int = PID_PARAMS['common']['max_integral']
        axis_state.integral = max(min(axis_state.integral, max_int), -max_int)
        i_term = axis_params['ki'] * axis_state.integral

        # D项: 微分控制
        derivative = (error - axis_state.prev_error) / dt
        d_term = axis_params['kd'] * derivative
        axis_state.prev_error = error

        # PID输出合成
        output = p_term + i_term + d_term
        print(output)
        # 转换为电机控制参数
        direction = 1 if output > 0 else 0

        # 速度计算 (基于输出幅度的非线性映射)
        abs_output = abs(output)
        # S型曲线速度映射 - 小误差时低速，大误差时高速
        min_spd = PID_PARAMS['common']['min_speed']
        max_spd = PID_PARAMS['common']['max_speed']

        # 非线性映射：小输出时缓慢增加速度，大输出时接近最大速度
        speed = min_spd + (max_spd - min_spd) * (1 - math.exp(-abs_output/20))
        speed = int(min(max(speed, min_spd), max_spd))

        # 脉冲数计算 (限制输出范围)
        scale = PID_PARAMS['common']['output_scale']
        pulses = int(abs_output * scale)
        pulses = min(max(pulses, PID_PARAMS['common']['min_pulses']),
                    PID_PARAMS['common']['max_pulses'])
#        if direction==1:
#            print(speed)
#        else:
#            print(-speed)
        return (direction, speed, pulses)

    # 计算各轴命令
    x_cmd = pid_controller(error_x, x_pid, PID_PARAMS['x'])
#    y_cmd = pid_controller(error_y, y_pid, PID_PARAMS['y'])
    y_cmd = (0,0,0)

    return x_cmd, y_cmd


LAST_PULSE_X = 0
LAST_PULSE_Y = 0

stop_flag = 0


# --------------------------- 主循环 ---------------------------
clock = time.clock()
image_shape = [sensor.height(), sensor.width()]  # [高, 宽] 用于cv_lite
while True:
    clock.tick()
    img = sensor.snapshot()


    '''
    # 1. 检测紫色色块（保留原有功能）
    purple_blobs = detect_purple_blobs(img)
    for blob in purple_blobs:
        img.draw_rectangle(blob[0:4], color=(255, 0, 255), thickness=1)
        img.draw_cross(blob.cx(), blob.cy(), color=(255, 0, 255), thickness=1)
    '''

    # 2. 矩形检测（使用cv_lite替换原有实现）
    # 2.1 将RGB图像转为灰度图（用于矩形检测）
    gray_img = img.to_grayscale()
    img_np = gray_img.to_numpy_ref()  # 转为numpy数组供cv_lite使用

    # 2.2 调用cv_lite矩形检测函数（带角点）
    rects = cv_lite.grayscale_find_rectangles_with_corners(
        image_shape,       # 图像尺寸 [高, 宽]
        img_np,            # 灰度图数据
        canny_thresh1,     # Canny低阈值
        canny_thresh2,     # Canny高阈值
        approx_epsilon,    # 多边形拟合精度
        area_min_ratio,    # 最小面积比例
        max_angle_cos,     # 角度余弦阈值
        gaussian_blur_size # 高斯模糊尺寸
    )

    # 3. 筛选最小矩形（保留原有逻辑）
    min_area = float('inf')
    smallest_rect = None
    smallest_rect_corners = None  # 存储最小矩形的角点

    for rect in rects:
        # rect格式: [x, y, w, h, c1.x, c1.y, c2.x, c2.y, c3.x, c3.y, c4.x, c4.y]
        x, y, w, h = rect[0], rect[1], rect[2], rect[3]
        # 提取四个角点
        corners = [
            (rect[4], rect[5]),   # 角点1
            (rect[6], rect[7]),   # 角点2
            (rect[8], rect[9]),   # 角点3
            (rect[10], rect[11])  # 角点4
        ]

        # 验证矩形有效性
        if is_valid_rect(corners):
            # 计算面积
            area = w * h  # 直接使用矩形宽高计算面积（更高效）
            # 更新最小矩形
            if area < min_area:
                min_area = area
                smallest_rect = (x, y, w, h)
                smallest_rect_corners = corners

    # 4. 处理最小矩形（修改后：固定虚拟矩形方向）
    if smallest_rect and smallest_rect_corners:
        x, y, w, h = smallest_rect
        corners = smallest_rect_corners

        # 对矩形角点进行排序
        sorted_corners = sort_corners(corners)

        # 绘制矩形边框和角点
        for i in range(4):
            x1, y1 = sorted_corners[i]
            x2, y2 = sorted_corners[(i+1) % 4]
            img.draw_line(x1, y1, x2, y2, color=(255, 0, 0), thickness=2)
        for p in sorted_corners:
            img.draw_circle(p[0], p[1], 5, color=(0, 255, 0), thickness=2)

        # 计算并绘制矩形中心点
        rect_center = calculate_center(sorted_corners)
        rect_center_int = (int(round(rect_center[0])), int(round(rect_center[1])))
        img.draw_circle(rect_center_int[0], rect_center_int[1], 4, color=(0, 255, 255), thickness=2)




        # 计算矩形主方向角（仅用于参考，不再影响虚拟矩形方向）
        angle = get_rectangle_orientation(sorted_corners)

        # 【核心修改】移除自动切换方向逻辑，固定使用预设的虚拟矩形尺寸和方向
        # 固定虚拟矩形（不再根据实际宽高比切换）
        virtual_rect = [
            (0, 0),
            (RECT_WIDTH, 0),
            (RECT_WIDTH, RECT_HEIGHT),
            (0, RECT_HEIGHT)
        ]

        # 【核心修改】固定圆形半径参数（不再根据实际宽高比调整）
        radius_x = BASE_RADIUS
        radius_y = BASE_RADIUS

        # 【核心修改】固定虚拟中心（基于固定的宽高）
        virtual_center = (RECT_WIDTH / 2, RECT_HEIGHT / 2)

        # 在虚拟矩形中生成椭圆点集（映射后为正圆）
        virtual_circle_points = []
        for i in range(POINTS_PER_CIRCLE):
            angle_rad = 2 * math.pi * i / POINTS_PER_CIRCLE
            x_virt = virtual_center[0] + radius_x * math.cos(angle_rad)
            y_virt = virtual_center[1] + radius_y * math.sin(angle_rad)
            virtual_circle_points.append((x_virt, y_virt))

        # 计算透视变换矩阵并映射坐标
        matrix = get_perspective_matrix(virtual_rect, sorted_corners)
        if matrix:


            stop_flag = 1

            mapped_points = transform_points(virtual_circle_points, matrix)
            int_points = [(int(round(x)), int(round(y))) for x, y in mapped_points]

            '''
            # 绘制圆形
            for (px, py) in int_points:
                img.draw_circle(px, py, 2, color=(255, 0, 255), thickness=2)
            '''
            # 绘制圆心
            mapped_center = transform_points([virtual_center], matrix)
            if mapped_center:
                cx, cy = map(int, map(round, mapped_center[0]))
                #img.draw_circle(cx, cy, 3, color=(0, 0, 255), thickness=1)
                img.draw_cross(cx, cy, color=(0, 0, 255), thickness=4)
                img.draw_line(cx, cy, 160, 120, color=(255, 0, 0), thickness=5)
                #print(f"点坐标：x={cx}, y={cy}")


                (x_dir,x_speed,x_pulse),(y_dir,y_speed,y_pulse) = get_motor_commands(cx,cy)

                send_motor_position(uart_x, x_dir, x_speed, x_pulse)
                send_motor_position(uart_y, y_dir, y_speed, y_pulse)

                if x_speed == 0 and y_speed == 0:
                    LED.high()
                else:
                    pass


                img.draw_string_advanced(10, 40,  20, f"x_speed: {x_speed}", color=(255, 255, 255))
                img.draw_string_advanced(10, 70,  20, f"x_pulse: {x_pulse}", color=(255, 255, 255))
                img.draw_string_advanced(10, 100, 20, f"y_speed: {y_speed}", color=(255, 255, 255))
                img.draw_string_advanced(10, 130, 20, f"y_pulse: {y_pulse}", color=(255, 255, 255))
                #圆心的坐标自己在这处理 云台
                #-----------------------------PID 处理

                '''
                speed_x = 0
                direction_x = 0
                pulse_x = 0

                x_offset = 2

                error_x = 160 - x_offset - cx
                abs_error_x = abs(error_x)

                xifen = 256

                # 死区处理
                if abs_error_x <= 1:
                    speed_x = -1
                    xifen = 256

                    stop_flag = 0

                elif abs_error_x <= 5:
                    speed_x = 10
                    xifen = 200
                    pulse_x = 20


                elif abs_error_x <= 10:  # 位置式控制 小脉冲
                    speed_x = 15
                    xifen = 128
                    pulse_x = 40

                elif abs_error_x <= 15:  # 位置式控制 小脉冲
                    speed_x = 18
                    xifen = 128
                    pulse_x = 128

                elif abs_error_x <= 20:  # 位置式控制
                    speed_x = 40
                    xifen = 32
                    pulse_x = 300

                elif abs_error_x <= 35:  # 速度控制
                    speed_x = 60
                    xifen = 32
                    pulse_x = 5000
                elif abs_error_x <= 50:
                    speed_x = 65
                    xifen = 32
                    pulse_x = 5000
                elif abs_error_x <= 80:
                    speed_x = 80
                    pulse_x = 5000
                    xifen = 32
                else:
                    speed_x = 50
                    xifen = 16
                    pulse_x = 5000

                # 确定方向 (0=正方向，1=反方向)
                direction_x = 1 if error_x > 0 else 0

                if (LAST_PULSE_X >= 2000 and pulse_x <= 2000 )or speed_x == -1:
                    send_motor_stop(uart_x)
                    time.sleep(0.01)

#                set_motor_step_division(uart_x,xifen)
#                time.sleep(0.01)

                if speed_x != -1:
                    send_motor_position(uart_x, direction_x, speed_x, pulse_x)
                    time.sleep(0.01)

                LAST_PULSE_X = pulse_x



                # ----------------------------Y
                speed_y = 0
                direction_y = 0
                pulse_y = 0


                y_offset = 12

                error_y = 120 - y_offset - cy
                abs_error_y = abs(error_y)
                '''
                #if abs_error_y <= 35:
                    #set_motor_step_division(uart_y,200)
                #else:
                    #set_motor_step_division(uart_y,64)
                '''
                # 死区处理
                if abs_error_y <= 1:
                    speed_y = -1
                elif abs_error_y <= 5:
                    speed_y = 7
                    pulse_y = 10
                elif abs_error_y <= 10:  # 位置式控制 小脉冲
                    speed_y = 10
                    pulse_y = 20
                elif abs_error_y <= 15:
                    speed_y = 12
                    pulse_y = 32
                elif abs_error_y <= 20:  # 位置式控制
                    speed_y = 35
                    pulse_y = 1000
                elif abs_error_y <= 35:
                    speed_y = 45
                    pulse_y = 5000
                elif abs_error_y <= 50:  # 速度控制
                    speed_y = 55
                    pulse_y = 5000
                elif abs_error_y <= 80:  # 速度控制
                    speed_y = 60
                    pulse_y = 5000
                else:
                    speed_y = 70
                    pulse_y = 5000



                # 确定方向 (0=正方向，1=反方向)
                direction_y = 1 if error_y > 0 else 0

                if (LAST_PULSE_Y >= 2000 and pulse_y <= 2000) or speed_y == -1:
                    send_motor_stop(uart_y)
                    time.sleep(0.01)


                if speed_y != -1:
                    send_motor_position(uart_y, direction_y, speed_y, pulse_y)
                    time.sleep(0.01)
                LAST_PULSE_Y = pulse_y



                if abs_error_x <= 1 and abs_error_y <= 1:
                    LED.high()

                '''
                #圆心的坐标自己在这处理 云台

            # 发送坐标
            #send_circle_points(int_points)

    else:
        pass
        '''
        if stop_flag == 1:
            stop_flag = 0
            #LED.low()
            send_motor_stop(uart_x)
            time.sleep(0.01)
            send_motor_stop(uart_y)
            time.sleep(0.01)
        '''

    # 5. 显示与性能统计
    fps = clock.fps()
    img.draw_string_advanced(10, 10, 20, f"FPS: {fps:.1f}", color=(255, 255, 255))  # 显示FPS

    # 显示图像
    Display.show_image(img,
                      x=round((lcd_width-sensor.width())/2),
                      y=round((lcd_height-sensor.height())/2))

#    print(f"FPS: {fps:.1f}")  # 打印FPS
